#include "g_code.h"
#include "tools.h"
#include "sys.h"
#include "interpolator.h"
// #define USE_PC_DEBUG

#ifdef USE_PC_DEBUG
#include "stdio.h"
#endif

unsigned long long g_code_current_line = 0;


extern g_code_handler_t g_handler[];
extern g_code_handler_t m_handler[];
extern float g_code_feed_rate;
extern ToolsInfo_t SystemToolsInfo;
extern g_code_sys_status_t g_code_interpolator;

DDA_Interpolator_t MainInterpolator={.flags = 0x01};
int g_code_translator_flags;

#define G_COMMAND_REGISTER_SIMPLE_INTEGER(command, src, target, param) \
if(((target->flags) & G_CODE_FLAGS_##command##_BIT) != G_CODE_FLAGS_##command##_BIT) \
{ \
    target->param = src->i_value.value; \
    target->flags |= G_CODE_FLAGS_##command##_BIT; \
} \
else \
{ \
    target->flags |= G_CODE_FLAGS_REPEAT_ERR_BIT; \
} \

#define G_COMMAND_REGISTER_SIMPLE_FLOAT(command, src, target, param) \
if(((target->flags) & G_CODE_FLAGS_##command##_BIT) != G_CODE_FLAGS_##command##_BIT) \
{ \
    target->param = src->f_value; \
    target->flags |= G_CODE_FLAGS_##command##_BIT; \
} \
else \
{ \
    target->flags |= G_CODE_FLAGS_REPEAT_ERR_BIT; \
} \

g_code_command_t command_list[12];
cx_uint32_t g_code_extract_flags = 0;
#define G_CODE_EXTRACT_MATCH 0x00000001

int g_code_g_handler_hashfunc(int G_NUMBER);
int g_code_m_handler_hashfunc(int M_NUMBER);

extern g_code_handler_t g_handler[];
extern g_code_handler_t m_handler[];

static char g_command_table[] = 
{
    'G','M', 
    'N',
    'T',
    'O',
    'L','Q',
    'H','P', /*float after*/ 'D',
    'X','Y','Z',
    'I','J','K',
    'R',
    'S','F',
    'U','V','W'
};
struct
{
    g_code_handler_t g_instr;
    g_code_handler_t m_instr;
}g_code_last_handler;

#ifdef USE_PC_DEBUG
const char g_code_flags_char[]=
{
    'G','M','T','X','Y','Z','I','J','K','R','F','S','N','L','Q','D','H','O','U','V','W'
};
#endif

static char * g_code_get_block(char *src, g_code_command_t *command)
{
    char *ret = CX_NULL, *cur = src;
    union{
        float f_val;
        struct 
        {
            cx_uint16_t val;
            cx_uint16_t sub_val;
        }i_val;
        
    }temp;
    cx_uint8_t float_num = 0;
    cx_uint8_t passive_flag = 0;
    g_code_extract_flags = 0;
    while((*cur != '\0') && (*cur != '\r') && (*cur != '\n'))
    {
        for(int i=0; i<sizeof(g_command_table); i++)
        {
            if(*cur == g_command_table[i])
            {
                g_code_extract_flags |= G_CODE_EXTRACT_MATCH;
                command->cmd = *cur;
                cur ++;
                if((*cur) == '-')
                {
                    passive_flag = 1;
                    cur++;
                }
                if(!((*cur) >= '0' && (*cur) <= '9'))
                {
                    command->cmd = '\0';
                    return CX_NULL;
                }
                if(i < 9) //整数个数
                {
                    temp.i_val.val =0;
                    temp.i_val.sub_val = 0;
                    while(((*cur) >= '0') && ((*cur) <= '9'))
                    {
                        temp.i_val.val *= 10;
                        temp.i_val.val += (*cur) - '0';
                        cur++;
                    }
                    command->i_value.value = temp.i_val.val;
                    if(passive_flag)
                    {
                        command->i_value.value = 0-command->i_value.value;
                    }
                }
                else
                {
                    float_num  = 0;
                    temp.f_val = 0;
                    while(((*cur) >= '0') && ((*cur) <= '9'))
                    {
                        temp.f_val *= 10;
                        temp.f_val += (*cur) - '0';
                        cur++;
                    }
                    if((*cur) == '.')
                    {
                        cur++;
                        for(float_num = 0; (((*cur) >= '0') && ((*cur) <= '9')) ; float_num++)
                        {
                            temp.f_val *= 10;
                            temp.f_val += (*cur) - '0';
                            cur++;
                        }
                        for(int i =0; i< float_num ; i++)
                        {
                            temp.f_val *= 0.1f;
                        }
                    }
                    command->f_value = temp.f_val;
                    if(passive_flag)
                    {
                        command->f_value = 0-command->f_value;
                    }
                }
                if(command->cmd == 'G')
                {
                    if((*cur) == '.')
                    {
                        cur++;
                        while(((*cur) >= '0') && ((*cur) <= '9'))
                        {
                            temp.i_val.sub_val *= 10;
                            temp.i_val.sub_val += (*cur) - '0';
                            cur++;
                        }
                        command->i_value.sub_value = temp.i_val.sub_val;
                        command->i_value.sub_value |= 0x8000;
                    }
                    ret = cur;
                }
                else
                {
                    ret = cur;
                }
                return ret;
            }
        }
        if(!(g_code_extract_flags & G_CODE_EXTRACT_MATCH))
        {
            if(*cur !=' ')
            {
                ret = CX_NULL;
                command->cmd = '\0';
                break;
            }
        }
        cur ++;
    }
    return ret;

}

cx_uint8_t g_code_get_command(char *g_code_line, g_code_command_t *command_array)
{
    cx_uint8_t result_num = 0;
    char *get_block_ret = g_code_line;
    get_block_ret = g_code_get_block(get_block_ret, &(command_array[result_num]));
    while(get_block_ret != CX_NULL)
    {
        result_num++;
        get_block_ret = g_code_get_block(get_block_ret, &(command_array[result_num]));
    }
    
    return result_num;
}

static inline void command_to_param_handler(g_code_command_t *src, g_code_param_t *target)
{
    switch (src->cmd)
    {
#ifdef G_COMMAND_TO_PARAM
    case 'G': 
    if(((target->flags) & G_CODE_FLAGS_G_BIT) != G_CODE_FLAGS_G_BIT)
    {
        target->g = src->i_value.value;
        target->flags |= G_CODE_FLAGS_G_BIT;
        if(src->i_value.sub_value & 0x8000) //G Type Sub-Group used for 5-Axis platform
        {
            target->sub_g = (src->i_value.sub_value) & 0x7fff;
            target->flags |= G_CODE_FLAGS_G_SUB_GROUP_BIT;
        }
    }
    else
    {
        target->flags |= G_CODE_FLAGS_REPEAT_ERR_BIT;
    }
    break;
#endif
#ifdef M_COMMAND_TO_PARAM
    case 'M': 
        G_COMMAND_REGISTER_SIMPLE_INTEGER(M,src,target,m);
    break;
#endif
    case 'T': 
        G_COMMAND_REGISTER_SIMPLE_INTEGER(T,src,target,t);
    break;
    case 'X': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(X,src,target,x);
    break;
    case 'Y': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(Y,src,target,y);
    break;
    case 'Z': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(Z,src,target,z);
    break;
    case 'I': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(I,src,target,i);
    break;
    case 'J': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(J,src,target,j);
    break;
    case 'K': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(K,src,target,k);
    break;
    case 'R': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(R,src,target,r);
    break;
    case 'F': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(F,src,target,f);
    break;
    case 'S': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(S,src,target,s);
    break;
    case 'U': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(U,src,target,u);
    break;
    case 'V': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(V,src,target,v);
    break;
    case 'W': 
        G_COMMAND_REGISTER_SIMPLE_FLOAT(W,src,target,w);
    break;
    default:
        break;
    }
}

cx_uint32_t g_code_get_param(char *g_code_line, g_code_param_t *ret_param)
{
    cx_uint8_t command_num;
    g_code_translator_flags &= ~G_CODE_TRANSLATOR_FLAG_OK_MASK;
    g_code_translator_flags |= G_CODE_TRANSLATOR_FLAG_OK_FALSE;
    ret_param->flags = 0;
    command_num = g_code_get_command(g_code_line, command_list);
    float * zerolizes = (float*)ret_param;
    for(int i = 0; i < 12; i++)
    {
        zerolizes[i]=0.0f;
    }
    ret_param->g = 0;
    ret_param->m = 0;
    ret_param->sub_g = -1;
    ret_param->other = CX_NULL;
    ret_param->flags = 0u;
    for(int i = 0; i < command_num; i++)
    {
        command_to_param_handler(&(command_list[i]), ret_param);
        switch (command_list[i].cmd)
        {
        case 'G':
            g_handler[command_list[i].i_value.value](ret_param);
            break;
        case 'M':
            m_handler[command_list[i].i_value.value](ret_param);
            break;
        case 'F':
            CNC_SetFeedSpeed(command_list[i].f_value);
            // system_feed_speed = command_list[i].f_value;
            break;
        case 'S':
            CNC_SetMainshaftSpeed(command_list[i].f_value);
            // system_spindle_speed = command_list[i].f_value;
            break;
        case 'T':
            SystemToolsInfo.NextTool = command_list[i].i_value.value;
            break;
        
        default:
            break;
        }
    }
    if((ret_param->flags & (G_CODE_FLAGS_X_BIT | G_CODE_FLAGS_Y_BIT | G_CODE_FLAGS_Z_BIT | G_CODE_FLAGS_I_BIT | G_CODE_FLAGS_J_BIT | G_CODE_FLAGS_R_BIT| G_CODE_FLAGS_K_BIT | G_CODE_FLAGS_U_BIT | G_CODE_FLAGS_V_BIT | G_CODE_FLAGS_W_BIT)) != 0)
    {
#define COORDINATE_UNIT_TO_PULSE_EQUIVALENT_UNIT(x,axis) ((IDTYPE)(x/system_pulse_equivalent_##axis))
// simplize input
#define COOR2PULSE(x,axis) COORDINATE_UNIT_TO_PULSE_EQUIVALENT_UNIT(x,axis)
        // g_code_interpolate_handler[g_code_interpolator](ret_param);
        switch (g_code_interpolator)
        {
            case G_CODE_INTERPOLATOR_QUICK:
                // break;
            case G_CODE_INTERPOLATOR_LINE:
                if(g_code_coords_mode == G_CODE_COORDS_MODE_ABSOLUTE)
                {
                    ret_param->x = ret_param->x - (System_RefCoordinate.x - System_ProgCoordinateOrigin.x)*system_pulse_equivalent_x;
                    ret_param->y = ret_param->y - (System_RefCoordinate.y - System_ProgCoordinateOrigin.y)*system_pulse_equivalent_y;
                    ret_param->z -= (System_RefCoordinate.z - System_ProgCoordinateOrigin.z)*system_pulse_equivalent_z;
                }
                DDA_line(&MainInterpolator,COOR2PULSE(ret_param->x,x),COOR2PULSE(ret_param->y,y),COOR2PULSE(ret_param->z,z));break;
            case G_CODE_INTERPOLATOR_CW:
                ret_param->i = 0-ret_param->i;
                ret_param->j = 0-ret_param->j;
                ret_param->x = ret_param->x + ret_param->i;
                ret_param->y = ret_param->y + ret_param->j;
                DDA_Arc(&MainInterpolator, COOR2PULSE(ret_param->i,x),COOR2PULSE(ret_param->j,y),COOR2PULSE(ret_param->x,x),COOR2PULSE(ret_param->y,y),ARC_DIR_CW);
                break;
            case G_CODE_INTERPOLATOR_CCW:
                ret_param->i = 0-ret_param->i;
                ret_param->j = 0-ret_param->j;
                ret_param->x = ret_param->x + ret_param->i;
                ret_param->y = ret_param->y + ret_param->j;
                DDA_Arc(&MainInterpolator, COOR2PULSE(ret_param->i,x),COOR2PULSE(ret_param->j,y),COOR2PULSE(ret_param->x,x),COOR2PULSE(ret_param->y,y),ARC_DIR_CCW);
                break;
            case G_CODE_INTERPOLATOR_SPIRAL:break;
            case G_CODE_INTERPOLATOR_SPLINE:break;
            case G_CODE_INTERPOLATOR_SET_REFERENCE_COOR:
                System_ProgCoordinateOrigin.x = System_RefCoordinate.x - ret_param->x;
                System_ProgCoordinateOrigin.y = System_RefCoordinate.y - ret_param->y;
                System_ProgCoordinateOrigin.z = System_RefCoordinate.z - ret_param->z;
                g_code_interpolator = G_CODE_INTERPOLATOR_LINE;
                break;
            case G_CODE_INTERPOLATOR_BUILD_RADIUS_COMPENSATION:break;
            default:
                break;
        }
#undef COOR2PULSE

#ifdef USE_PC_DEBUG
        printf("x:%f,y:%f,z:%f\ni:%f,j:%f,k:%f\nu:%f,v:%f,w:%f\n",  ret_param->x,ret_param->y,ret_param->z,
                                                                    ret_param->i,ret_param->j,ret_param->k,
                                                                    ret_param->u,ret_param->v,ret_param->w);
#endif
    }
#if defined(G_COMMAND_TO_PARAM) || defined(M_COMMAND_TO_PARAM)
    switch((ret_param->flags) & G_CODE_FLAGS_FUNC_INSTRUMENT_MASK)
    {
        case 0:g_code_last_handler.g_instr(ret_param); break;
#ifdef G_COMMAND_TO_PARAM
        case G_CODE_FLAGS_G_BIT:g_code_last_handler.g_instr=g_handler[g_code_g_handler_hashfunc(ret_param->g)];
            g_code_last_handler.g_instr(ret_param);break;
#endif
#ifdef M_COMMAND_TO_PARAM
        case G_CODE_FLAGS_M_BIT:g_code_last_handler.m_instr=m_handler[g_code_m_handler_hashfunc(ret_param->m)];
            g_code_last_handler.m_instr(ret_param);break;
#endif
        default:command_num = 0;break;
    }
#endif
    g_code_translator_flags &= ~G_CODE_TRANSLATOR_FLAG_OK_MASK;
    g_code_translator_flags |= G_CODE_TRANSLATOR_FLAG_OK_TRUE;
    return command_num;
}

// #define USE_PC_DEBUG
#ifdef USE_PC_DEBUG
#include <stdio.h>
#include<string.h>
void interpolator_Handler(void);
int main(void)
{
    g_code_last_handler.m_instr = m_handler[0];
    g_code_last_handler.g_instr = g_handler[0];
    FILE *file;
    g_code_param_t g_code_;
    char g_code_line_[100];
    if((file = fopen("./point.txt","w+")) == NULL)
    {
        return -1;
    }
    g_code_handler_init();
    while(1)
    {
        gets(g_code_line_);
        if(strcmp(g_code_line_,"exit") == 0)
        {
            break;
        }
        g_code_get_param(g_code_line_,&g_code_);
        printf("Code:%s\nflags:%x\n", g_code_line_,g_code_.flags);

        for(int i=0; !(MainInterpolator.flags & 1UL); i++)
        {
            interpolator_Handler();
            fprintf(file, "%d,%d,%d\n", System_RefCoordinate.x,System_RefCoordinate.y,System_RefCoordinate.z);
        }
    }
    fclose(file);
    return 0;
}
#endif

/**example**/
/*

cx_uint16_t g_code_phaser(char *code_line ,g_code_handler_t handler)
{
#define object_written current_g_param
#define translate_result trans_result
    cx_uint8_t result_num;
    char *get_block_ret = code_line;
    for(result_num = 0; get_block_ret != CX_NULL; result_num++)
    {
        get_block_ret = g_code_get_block(get_block_ret, &(translate_result[result_num]));
    }
    for(int i = 0; i < result_num; i++)
    {
        switch(translate_result[i].cmd)
        {
            case 'G':object_written.g = translate_result[i].i_value; object_written.flags &= ~G_CODE_FLAG_MODE_M;break;
            case 'M':object_written.m = translate_result[i].i_value; object_written.flags |= G_CODE_FLAG_MODE_M;break;
            case 'X':object_written.x = translate_result[i].f_value;break;
            case 'Y':object_written.y = translate_result[i].f_value;break;
            case 'Z':object_written.z = translate_result[i].f_value;break;
            case 'I':object_written.i = translate_result[i].f_value;break;
            case 'J':object_written.j = translate_result[i].f_value;break;
            case 'K':object_written.k = translate_result[i].f_value;break;
            case 'R':object_written.r = translate_result[i].f_value;break;
            case 'F':object_written.f = translate_result[i].f_value;break;
            case 'S':object_written.s = translate_result[i].f_value;break;

            default:
            //Unsupported
            break;
        }
    }
    handler(&object_written);
    object_written.flags &= ~G_CODE_FLAG_MODE_M;
    return result_num;
    
#undef object_written
#undef translate_result
}

#ifdef USE_PC_DEBUG
void g_code_cb(g_code_parm_t *param)
{
    printf("Mode:%c,\nX:%f,\nY:%f,\nZ:%f,\nI:%f,\nJ:%f,\nK:%f,\nR:%f,\nF:%f,\nS:%f,\n", 
    ((param->flags & G_CODE_FLAG_MODE_MASK) == G_CODE_FLAG_MODE_M)?('M'):('G'),
    // param->flags,
    param->x,
    param->y,
    param->z,
    param->i,
    param->j,
    param->k,
    param->r,
    param->f,
    param->s);
}

int main(void)
{
    char input[120];
    while(1)
    {
        scanf("%s",input);
        g_code_phaser(input, g_code_cb);
    }
    return 0;

}

#endif

*/